package movement;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.DifferentialPilot;
import navigation.Navigation;
import protocol.Protocol.Correction;
import protocol.Protocol.Direction;
import protocol.Protocol.Field;
import sensors.Grid;

public class Engine {

	private DifferentialPilot pilot;
	private Grid grid = new Grid();
	private boolean firstCorrection = false;
	private double correctionFactor;
	private int correctionCounter = 0;

	public Engine(double one, double two) {
		pilot = new DifferentialPilot(one, two, Motor.B, Motor.C);
	}

	/**
	 * The agent moves the Distance
	 * 
	 * @param distance
	 *            The desired Distance
	 */
	public void move(double distance) {
		pilot.travel(distance);
	}

	/**
	 * The Agent rotates
	 * 
	 * @param angle
	 *            The desired angle
	 */
	public void rotate(double angle) {
		pilot.rotate(angle);
	}

	/**
	 * Explores the desired field
	 * 
	 * @param returnToOldField
	 *            True if the agent should return to his starting field after
	 *            exploring
	 * @return
	 */
	public Field explore(boolean returnToOldField) {

		moveForwardtoFieldEnd();

		// Not available as long as library isn't provided by the com team
		// while (!communication.isMoveOK) {
		// Thread.sleep(5000);
		// }

		return moveIntoNewField(returnToOldField);

	}

	/**
	 * Moves to the end of the field and corrects its position
	 */
	private void moveForwardtoFieldEnd() {

		// Motor.B.forward();
		// Motor.C.forward();

		LCD.drawString("D: " + Navigation.currentDirection, 0, 5);

		int color = grid.getColor();
		int color2 = grid.getColor2();

		int firstColor = color;
		int light = grid.getBrightness();

		firstCorrection = true;

		// Colorcode 6 is a bias caused by bad light therefore filter it while
		// searching for the field limitation
		while (color != Grid.red && color != Grid.green && color2 != Grid.red
				&& color2 != Grid.green || (color == 6 || color2 == 6)) {
			// while (color != Grid.red && color != Grid.green) {// &&
			// !grid.green(light)
			// &&
			// !grid.red(light))
			// {
			pilot.travel(0.3);
			color = grid.getColor();
			color2 = grid.getColor2();
			// light = grid.getBrightness();
		}
		LCD.drawString("Sehe: " + color + " und " + color2, 0, 5);
		try {
			Thread.sleep(2000);
			LCD.clear();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		// Motor.B.stop();
		// Motor.C.stop();

		if ((color == Grid.green && color2 == Grid.green)
				|| (color == Grid.red && color2 == Grid.red)) {
			correctionFactor = correctionFactor * 0.75;
			firstCorrection = true;
			correctionCounter = 0;
			return;
		} else {

			// linker Sensor ist an S1 (von hinten gesehen)
			if (firstColor == color) {
				if ((Navigation.currentDirection == Direction.NORTH || Navigation.currentDirection == Direction.SOUTH)
						&& color2 == Grid.red) {
					correct(Correction.Right);
				} else if ((Navigation.currentDirection == Direction.NORTH || Navigation.currentDirection == Direction.SOUTH)
						&& color2 == Grid.green) {
					correct(Correction.Left);
				} else if ((Navigation.currentDirection == Direction.EAST || Navigation.currentDirection == Direction.WEST)
						&& color2 == Grid.green) {
					correct(Correction.Right);
				} else {
					correct(Correction.Left);
				}

			} else {
				if ((Navigation.currentDirection == Direction.NORTH || Navigation.currentDirection == Direction.SOUTH)
						&& color == Grid.red) {
					correct(Correction.Left);
				} else if ((Navigation.currentDirection == Direction.NORTH || Navigation.currentDirection == Direction.SOUTH)
						&& color == Grid.green) {
					correct(Correction.Right);

				} else if ((Navigation.currentDirection == Direction.EAST || Navigation.currentDirection == Direction.WEST)
						&& color2 == Grid.green) {
					correct(Correction.Right);
				} else if ((Navigation.currentDirection == Direction.EAST || Navigation.currentDirection == Direction.WEST)
						&& color2 == Grid.red) {
					correct(Correction.Left);
				} else if ((Navigation.currentDirection == Direction.EAST || Navigation.currentDirection == Direction.WEST)
						&& color2 == Grid.free && color == Grid.red) {
					correct(Correction.Right);
				} else {
					correct(Correction.Left);
				}
			}
		}

	}

	/**
	 * Corrects the Alignment of the Agent and stores the correction Information
	 * for further usage
	 * 
	 * @param correction
	 *            The desired correction (left or right)
	 */
	private void correct(Correction correction) {

		pilot.travel(-1);
		if (correctionCounter < 5) {

			if (correction == Correction.Right) {
				if (firstCorrection) {
					rotate(2.5 + correctionFactor);
					firstCorrection = false;
				} else {
					rotate(2.5);
					correctionFactor += 2.5;

				}
			} else {
				if (firstCorrection) {
					rotate(-2.5 + correctionFactor);
					firstCorrection = false;
				} else {
					rotate(-2.5);
					correctionFactor += -2.5;

				}
			}
		} else {
			if (correction == Correction.Right) {
				if (firstCorrection) {
					rotate(1.5 + correctionFactor);
					firstCorrection = false;
				} else {
					rotate(1.5);
					correctionFactor += 1.5;

				}
			} else {
				if (firstCorrection) {
					rotate(-1.5 + correctionFactor);
					firstCorrection = false;
				} else {
					rotate(-1.5);
					correctionFactor += -1.5;

				}
			}
		}
		moveForwardtoFieldEnd();

	}

	/**
	 * The Agent moves into the new Field and explores it
	 * 
	 * @param returnToOldField
	 *            True if the agent should return to his starting point
	 * @return The type of the explored Field
	 */
	private Field moveIntoNewField(boolean returnToOldField) {

		for (int i = 0; i < 25; i++) {
			if (grid.wallDetected() != Field.FREE) {
				Field newField = grid.wallDetected();
				pilot.travel(-0.5 * i * 1.5);
				return newField;
			}
			pilot.travel(0.5);
		}

		if (grid.getColor() == Grid.free) {
			if (returnToOldField) {
				pilot.travel(-15);
			}
			return Field.FREE;
		} else {
			if (returnToOldField) {
				pilot.travel(-15);
			}
			return Field.TARGET;
		}

	}

}
